package com.grt192.controller;

import com.grt192.actuator.spot.GRTDemoLED;
import com.grt192.core.controller.EventController;
import com.grt192.mechanism.CreateBase;
import com.grt192.mechanism.SPOTDriverStation;
import com.grt192.mechanism.component.DriverStationListener;
import com.grt192.mechanism.spot.SLight;
import com.grt192.sensor.spot.NetServantSensor;
import com.grt192.utils.Util;

/**
 * Arcade style drive of a CreateBase given a single DemoJoystick, sends percent
 * events
 * 
 * @author Andrew Chen <andrewtheannihilator@gmail.com>
 */
public class CreateTeleopController extends EventController implements
		DriverStationListener {

	private static final double DEADZONE = .2;
	private static final double G = 1.5;
	private static final int threadLight = 6;
	private static final int xLight = 4;
	private static final int yLight = 5;
	private final CreateBase base;
	private final SPOTDriverStation ds;
	private double x, y;

	/**
	 * 
	 * @param base
	 *            base to drive
	 * @param ds
	 *            driver station to read Joystick from
	 */
	public CreateTeleopController(CreateBase base, SPOTDriverStation ds) {
		this.base = base;
		this.ds = ds;
		SLight.get(threadLight).rawColor(GRTDemoLED.Color.ORANGE);
	}

	public void run() {
		SLight.get(threadLight).rawColor(GRTDemoLED.Color.GREEN);
		// base.getAngleEncoder().addSensorChangeListener(this);
		running = true;
		while (running) {
			// drive();// act
			// log("NUM-COMMANDS:" + base.getDT().getNumCommands());

			// log("velocity:"
			// + base.getVelocityEncoder().getState(
			// EncoderVelocity.LEFT_VELOCITY)
			// + "\t"
			// + base.getVelocityEncoder().getState(
			// EncoderVelocity.RIGHT_VELOCITY));
			Util.sleep(100);
		}
	}

	public void startListening() {
		ds.addDSListener(this);
	}

	public void stopListening() {
		ds.removeDSListener(this);
	}

	/**
	 * Transforms a percentage control for more exotic controls schemes(ie.
	 * square)
	 * 
	 * @param percent
	 *            linear percent read from joystick [-1.0... 1.0]
	 * @return transformed percent [-1.0 ... 1.0]
	 */
	private static double convertControl(double percent) {
		double mult = percent <= 0 ? -1.0 : 1.0;
		return (percent * percent) * mult;// squared
	}

	public void joystickMoved(NetServantSensor joystick, double x, double y) {
		SLight.get(threadLight).blinkToBlack();
		double vX = convertControl(x);
		double vY = convertControl(y);
		// log("Drive: " + (vY - vX) + "\t " + vY + vX);
		base.percentDrive(vY - vX, vY + vX);
	}
}
